package org.mashart.pink.m.gps;

import org.mashart.pink.m.gps.data.NMEAAssembler;
import org.mashart.pink.m.ws.WsConnector;

/**
 * Buffer for the Waypoints to be sent out.
 * Store the latest waypoints from receiver, and decide whether to send the data immediately.
 */
public class WaypointsBuffer {

    /**
     * Max interval between sending two points
     */
    private final int MAX_INTERVAL = 30;
    /**
     * Default realative threshold on difference between old and current velocity.
     * Need to be ajusted by looking at the real application.
     */
    private final float DEFAULT_REALATIVE_THRESHOLD = (float) 0.4;
    /**
     * To compare with altitude (meter), latitude (degree) need to be converted: (lat * PI / 180) * Earth_Radius.
     * The other way around, to compare with latitude, altitude need to be converted:
     * (alt / Earth_Radius) * 180 / PI = alt * 180 /  (Earth_Radius * PI)
     */
    private final double ALT_SCALE = 180 / (6378137 * Math.PI);
    /**
     * Arbitary weight on vertical velocity. E.g, ALT_WEIGHT = 5 means change on
     * vertical velocity by 1m/s is considered as significant as change on horizontal
     * velocity by 5m/s.
     */
    private final int WEIGHT_VERTICAL = 5;
    /**
     * Previous received Waypoint
     */
    private Waypoint preWp;
    /**
     * Approximate distance scale of longitude over latitude.
     * That is, distance of moving towards north/south by 1 degree / distance of moving towards east/west by 1 degree
     */
    private double lonScale;
    /**
     * Old velocity on longitude
     */
    private double oldVelocityLon;
    /**
     * Old velocity on latitude
     */
    private double oldVelocityLat;
    /**
     * Old velocity on altitude
     */
    private double oldVelocityAlt;
    /**
     * Relative threshold on difference between old and current velocity
     */
    private float relativeThres;
    /**
     * Distance threshold, equals realativeThres * ( |v_lon| + |v_lat| )
     */
    private double distThres;
    /**
     * If the previous waypoint is sent
     */
    private boolean preWpIsSent;
    /**
     * counter, the number of skipped waypoints since last sending
     */
    private int counter;

    /**
     * Initialize
     */
    public int initialize() {
        // initialize
        lonScale = 0.0;
        oldVelocityLon = 0.0;
        oldVelocityLat = 0.0;
        oldVelocityAlt = 0.0;
        distThres = 0.0;
        preWpIsSent = false;
        counter = 0;
        // set the threshold to default value
        relativeThres = DEFAULT_REALATIVE_THRESHOLD;
        return 1;
    }

    /**
     * Set threshold. Current can only be called before inserting any waypoint.
     */
    public void setThreshold(float threshold) {
        relativeThres = threshold;
    }

    /**
     * Get threshold
     */
    public float getThreshold() {
        return relativeThres;
    }

    /**
     * Update with a new received waypoint.
     * The new received waypoint should have a newer timestamp than the previous ones,
     * otherwise, it is discarded.
     *
     */
    public void update(WsConnector ws_client, Waypoint wp) {
        // waypoint counter
        counter++;
        // distance between old and current velocity
        double dist;

        // invalid wp
        if (wp == null) {
            return;
        }

        // if it is the first received waypoint
        if (preWp == null) {
            // get a latitude
            double lat = wp.latitude;
            lonScale = Math.cos(Math.toRadians(lat));
        } else {
            // calculate the elapsed time from previous waypoint to the new received one
            int t = NMEAAssembler.diffTime(wp.timeMS, preWp.timeMS);
            // the new received waypoint is older than the previous one, this should not happen
            if (t <= 0) {
                return;
            }
            // the velocity along longitude need to be amended using lonScale, i.e., 1 degree along longitude is as long as along latitude
            double curVelocityLon = (wp.longitude - preWp.longitude) * lonScale / t;
            double curVelocityLat = (wp.latitude - preWp.latitude) / t;
            double curVelocityAlt;
            // check if the current velocity along altitude can be calculated
            if ( ! Float.isNaN(wp.altitude) && ! Float.isNaN(preWp.altitude) ) {
                curVelocityAlt = (wp.altitude - preWp.altitude) * ALT_SCALE * WEIGHT_VERTICAL / t;
            } else {
                // if the current velocity along altitude can NOT be calculated, assume it is not changed
                curVelocityAlt = oldVelocityAlt;
            }

            // calculate the distance between two velocity vectors
            dist = distance(curVelocityLon, curVelocityLat, curVelocityAlt, oldVelocityLon, oldVelocityLat, oldVelocityAlt);

            // if the previous waypoint is a turning point
            if (dist > distThres) {

                // if the previous waypoint (the turning point) is NOT sent in last update, send it now
                if (!preWpIsSent) {
                    // DEBUG
//                    System.out.print("[" + (System.currentTimeMillis() % 1000000) + "]");
//                    System.out.println("Turning WP");
                    ws_client.push(preWp.toHttpQuery());
                }
                // DEBUG
//                System.out.print("[" + (System.currentTimeMillis() % 1000000) + "]");
//                System.out.println("Following WP");
                // send current waypoint (the waypoint following the turning point)
                ws_client.push(wp.toHttpQuery());
                // remember this preWp is sent
                preWpIsSent = true;
                // reset the counter
                counter = 0;

                // update the old velocity
                oldVelocityLon = curVelocityLon;
                oldVelocityLat = curVelocityLat;
                oldVelocityAlt = curVelocityAlt;

                // calculate the referenced length, i.e., |v_lon| + |v_lat|
                double ref = distance(curVelocityLon, curVelocityLat, curVelocityAlt, 0, 0, 0);
                // update the distance threshold
                distThres = ref * relativeThres;
            } // the max interval is reached
            else if (counter == MAX_INTERVAL) {
                // DEBUG
//                System.out.print("[" + (System.currentTimeMillis() % 1000000) + "]");
//                System.out.println("Alive WP");
                // send current waypoint (the waypoint following the turning point)
                ws_client.push(wp.toHttpQuery() + "&alive=1");
                // remember this preWp is sent
                preWpIsSent = true;
                // reset the counter
                counter = 0;
            } else {
                // remember this preWp is NOT sent
                preWpIsSent = false;
            }
        }

        // after processing, save the waypoint for the next update.
        preWp = wp;
    }

    /**
     * Terminate the waypoint buffer. Send the last point, and close the web service connection.
     */
    public void terminate(WsConnector ws_client) {
        if (preWp != null) {
            // DEBUG
//            System.out.print("[" + (System.currentTimeMillis() % 1000000) + "]");
//            System.out.println("Final WP");
            ws_client.push(preWp.toHttpQuery());
            preWp = null;
        }
    }

    /**
     * Calculate the distance between two vectors.
     * Manhatton distance is currently used.
     */
    private double distance(double x1, double y1, double z1, double x2, double y2, double z2) {
        return Math.abs(x1 - x2) + Math.abs(y1 - y2) + Math.abs(z1 - z2);
    }
}
